/*
 *	Filename: Quadrotor_Mpu6050.c
 *	Fuction: Receive Sensor Data from Mpu6050
 *	Modified: 2016-11-09
 *	Author: Bin
 */

#include "DSP28x_Project.h"
#include "Quadrotor_Mpu6050.h"
#include <math.h>

void InitI2cGpio(void) {
	EALLOW;

	GpioCtrlRegs.GPBPUD.bit.GPIO32 = 0;		// Enable pull-up for GPIO32 (SDAA)
	GpioCtrlRegs.GPBPUD.bit.GPIO33 = 0;		// Enable pull-up for GPIO33 (SCLA)

	GpioCtrlRegs.GPBQSEL1.bit.GPIO32 = 3;	// Asynch input GPIO32 (SDAA)
	GpioCtrlRegs.GPBQSEL1.bit.GPIO33 = 3;	// Asynch input GPIO33 (SCLA)

	GpioCtrlRegs.GPBMUX1.bit.GPIO32 = 1;	// Configure GPIO32 for SDAA operation
	GpioCtrlRegs.GPBMUX1.bit.GPIO33 = 1;	// Configure GPIO33 for SCLA operation

	EDIS;
}

// Prescale Settings
void InitI2c(void) {
	// Make Sure in Reset Mode
	// Initialize I2C-A:
	I2caRegs.I2CPSC.all = 14;	// div_15, 150to10 MHz
	I2caRegs.I2CCLKL = 10;		// lowDiv_15
	I2caRegs.I2CCLKH = 5;		// highDiv_10

	I2caRegs.I2CMDR.bit.IRS = 1;// Take I2C out of reset

	// Disable FIFO
	// I2caRegs.I2CFFTX.all = 0x6000;
	// I2caRegs.I2CFFRX.all = 0x2040;
}

// Basic I2c Operation to Write Slave Device
Uint16 I2cWrite(Uint16 i2cAddr, Uint16 data) {
	//Waiting Time Counter
	Uint16 counter;
	// Wait for Stop condition
	counter = 0;
	while(I2caRegs.I2CMDR.bit.STP == 1) {
		if(counter++ == MAX_WAIT_TIME)
			return I2C_STP_NOT_READY_ERROR;
		DELAY_US(1);
	}	
	// Wait for bus idle
	counter = 0;
	while(I2caRegs.I2CSTR.bit.BB == 1) {
		if(counter++ == MAX_WAIT_TIME)
			return I2C_BUS_BUSY_ERROR;
		DELAY_US(1);
	}
	// Set Slave Address
	I2caRegs.I2CSAR = i2cAddr;
	// Set Data count
	I2caRegs.I2CCNT = 1;

	// Send Start as Master Transmitter
	I2caRegs.I2CMDR.all = 0x6E20;
	// Transmit Data
	I2caRegs.I2CDXR = data;

	return I2C_SUCCESS;
}

// Basic I2c Operation to Read Slave Device
Uint16 I2cRead(Uint16 i2cAddr, Uint16 *pData) {
	Uint16 counter;
	// Wait for Stop condition
	counter = 0;
	while(I2caRegs.I2CMDR.bit.STP == 1) {
		if(counter++ == MAX_WAIT_TIME)
			return I2C_STP_NOT_READY_ERROR;
		DELAY_US(1);
	}
	// Wait for bus idle
	counter = 0;
	while(I2caRegs.I2CSTR.bit.BB == 1) {
		if(counter++ == MAX_WAIT_TIME)
			return I2C_BUS_BUSY_ERROR;
		DELAY_US(1);
	}
	// Set Slave Address
	I2caRegs.I2CSAR = i2cAddr;
	// Set Data Counter
	I2caRegs.I2CCNT = 1;

	// Get Start as Master Receiver with NACK
	I2caRegs.I2CMDR.all = 0xEC20;
	// Receive Data
	counter = 0;
	while(I2caRegs.I2CSTR.bit.RRDY != 1) {
		if(counter++ == MAX_WAIT_TIME)
			return I2C_ERROR;
		DELAY_US(1);
	}
	*pData = I2caRegs.I2CDRR;

	return I2C_SUCCESS;
}

Uint16 writeMpu6050(Uint16 regAddr, Uint16 data) {
	Uint16 counter;
	// Wait for Stop condition
	counter = 0;
	while(I2caRegs.I2CMDR.bit.STP == 1) {
		if(counter++ == MAX_WAIT_TIME)
			return I2C_STP_NOT_READY_ERROR;
		DELAY_US(1);
	}	
	// Wait for bus idle
	counter = 0;
	while(I2caRegs.I2CSTR.bit.BB == 1) {
		if(counter++ == MAX_WAIT_TIME)
			return I2C_BUS_BUSY_ERROR;
		DELAY_US(1);
	}
	// Set Slave Address
	I2caRegs.I2CSAR = MPU6050_ADDRESS;
	// Set Data count
	I2caRegs.I2CCNT = 2;

	// Send Start as Master Transmitter
	I2caRegs.I2CMDR.all = 0x6E20;
	// Transmit regAddr
	I2caRegs.I2CDXR = regAddr;
	// Transmit data
	counter = 0;
	while(I2caRegs.I2CSTR.bit.XRDY != 1) {
		if(counter++ == MAX_WAIT_TIME)
			return I2C_ERROR;
		DELAY_US(1);
	}
	I2caRegs.I2CDXR = data;

	return I2C_SUCCESS;
}

Uint16 readMpu6050(Uint16 regAddr, Uint16 *pData) {
	Uint16 error;
	error = I2cWrite(MPU6050_ADDRESS, regAddr);
	if(!error)
		error = I2cRead(MPU6050_ADDRESS, pData);
	return error;
}

Uint16 InitMpu6050(void) {
	Uint16 who = 0;
	writeMpu6050(PWR_MGMT_1, 0x00);
	writeMpu6050(SMPLRT_DIV, 0x07);
	writeMpu6050(CONFIG, 0x06);
	writeMpu6050(GYRO_CONFIG, 0x18);
	writeMpu6050(ACCEL_CONFIG, 0x01);

	readMpu6050(WHO_AM_I, &who);

	return who;
}

Uint16 getSensor(struct Mpu6050Data *p) {
	Uint16 dataH = 0, dataL = 0;
	int16 data = 0;
	float32 YZ = 0.0, XZ = 0.0;

	// Read ACC_X
	readMpu6050(ACCEL_XOUT_H, &dataH);
	readMpu6050(ACCEL_XOUT_L, &dataL);
	data = (dataH << 8) | (dataL & 0xFF);
	p->ACC_X = (float32)data / 16384.0f;
	// Read ACC_Y
	readMpu6050(ACCEL_YOUT_H, &dataH);
	readMpu6050(ACCEL_YOUT_L, &dataL);
	data = (dataH << 8) | (dataL & 0xFF);
	p->ACC_Y = (float32)data / 16384.0f;
	// Read ACC_Z
	readMpu6050(ACCEL_ZOUT_H, &dataH);
	readMpu6050(ACCEL_ZOUT_L, &dataL);
	data = (dataH << 8) | (dataL & 0xFF);
	p->ACC_Z = (float32)data / 16384.0f;
	// Read GYR_X
	readMpu6050(GYRO_XOUT_H, &dataH);
	readMpu6050(GYRO_XOUT_L, &dataL);
	data = (dataH << 8) | (dataL & 0xFF);
	p->GYR_X = (float32)data / 16.384f;
	// Read GYR_Y
	readMpu6050(GYRO_YOUT_H, &dataH);
	readMpu6050(GYRO_YOUT_L, &dataL);
	data = (dataH << 8) | (dataL & 0xFF);
	p->GYR_Y = (float32)data / 16.384f;
	// Read GYR_Z
	readMpu6050(GYRO_ZOUT_H, &dataH);
	readMpu6050(GYRO_ZOUT_L, &dataL);
	data = (dataH << 8) | (dataL & 0xFF);
	p->GYR_Z = (float32)data / 16.384f;

	// Calculate Roll angle and Pitch angle
	XZ = sqrt((p->ACC_X) * (p->ACC_X) + (p->ACC_Z) * (p->ACC_Z));
	YZ = sqrt((p->ACC_Y) * (p->ACC_Y) + (p->ACC_Z) * (p->ACC_Z));
	p->Roll = atan(p->ACC_Y / XZ) / PI * 180.0;
	p->Pitch = atan(p->ACC_X / YZ) / PI * 180.0;

	return 0;
}
